#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>


int main(int argc, char **argv) {
    
    Eigen::Matrix3d roration_matrix = Eigen::Matrix3d::Identity();
    cout<< roration_matrix<<endl;
    Eigen::AngleAxisd roration_vector (M_PI/4,Eigen::Vector3d(0,0,1));
    cout .precision(3);
    roration_matrix = roration_vector.toRotationMatrix();
    cout<<"(1,0,0) after roration"<<roration_matrix.transpose()<<endl;
    
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rorate = roration_vector*v;
    cout<<"(1,0,0) after roration driectly"<<v_rorate.transpose()<<endl;
    
    v_rorate = roration_matrix *v;
    cout<<"(1,0,0) after roration"<<v_rorate.transpose()<<endl;
    
    Eigen::Vector3d euler_angle = roration_matrix.eulerAngles(2,1,0);
    cout<<"yaw pitch roll"<<euler_angle.transpose()<<endl;
    
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(roration_vector);
    T.pretranslate(Eigen::Vector3d(1,3,4));
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;
    
    Eigen::Vector3d v_transformed = T*v;
    cout<<"v tranform = "<<v_transformed.transpose()<<endl;
    
    Eigen::Quaterniond q = Eigen::Quaterniond (roration_vector);
    cout <<" quaterniond = \n"<<q.coeffs()<<endl;
    
    q = Eigen::Quaterniond (roration_vector);
    cout << "quaterniond = \n "<<q.coeffs()<<endl;
    
    v_rorate = q*v;
    cout<<"(1,0,0) after roration"<<v_rorate.transpose()<<endl;
    
    std::cout << "Hello, world!" << std::endl;
    return 0;
}
